#ifndef __COMMANDER__
#define __COMMANDER__

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <errno.h>
#include <string.h>
#include <netdb.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <signal.h>
#include <unistd.h>
#include <pthread.h>
#include <sys/time.h>
#include <time.h> 
#include <stdarg.h>
#include <semaphore.h>

#include "../vision/thread_common_op.h"
#include "../pilot/autopilot_interface.h"


#if 1

#define SERVER_NAME		"121.199.53.63"
#define SERVER_PORT		(8888)

#else

#define SERVER_NAME		"120.26.64.251"
#define SERVER_PORT		(8888)

#endif


#define HTTP_REQ_SIZE		(512)
#define HTTP_RECV_BUF_SIZE	(1024)
#define HTTP_POST_DATA_SIZE	(1024)

#define XYI_UAV_ID(x)       x
#define XYI_UAV_SYSTEM      0


#define BUFFER_LENGTH 2041

#define GD2GE_LONGTI_DIFF	(0.004740)
#define GD2GE_LATI_DIFF		(-0.002450)

#define MAV_FRAME_LOCAL_NED 1
#define MAV_FRAME_GLOBAL    0

#define COMMAND_XYI_LONG    101
#define COMMAND_ACK         102
#define MISSION_ITEMS       105
#define MISSION_ACK         106
#define MISSION_INQ         107
#define MISSION_ACK_ROUTE   108
#define MISSION_ACK_EMER    109
#define EMER_REPORT_IMAGE   128

#define EMER_LAND_NUMBER    5


#define SYS_STAT_OFF_LINE        0
#define SYS_STAT_ON_LINE         1
#define SYS_STAT_ONLOAD          2
#define SYS_STAT_WAIT_FLY       52
#define SYS_STAT_IMAGE_IDENTIFY 55
#define SYS_STAT_TAKEOFF         3
#define SYS_STAT_GO_UP           4
#define SYS_STAT_P2P             5
#define SYS_STAT_GO_DOWN         6
#define SYS_STAT_DELIVER         7
#define SYS_STAT_GOBACK_UP       8
#define SYS_STAT_GOBACK_P2P      9
#define SYS_STAT_GOBACK_DOWN    10
#define SYS_STAT_LAND           11
#define SYS_STAT_MISSION_OK     18
#define SYS_STAT_FLY_WAIT       20
#define SYS_STAT_EMER_ROUTE     30
#define SYS_STAT_EMER_DOWN      31
#define SYS_STAT_EMER_LAND      32
#define SYS_STAT_MANUAL_CONTROL 33
#define SYS_STAT_REMOTE_CONTROL 34
class xy_command_c;
class xy_network_c;

typedef struct 
{
	int takeoff_flag;
	float ascend_rate;
	float yaw_angle;
}takeoff_t;

typedef struct 
{
	int land_flag;
	float number;
	float mode;
	float yaw_angle;
}emer_land_t;

typedef struct _pid_test
{
	int pid_flag;
	float p1;
	float p2;
	float p3;
	float p4;
	float p5;
	float p6;
	float p7;
}pid_test_t;

typedef struct 
{
	uint8_t msgid;
	uint8_t target_uav;
	uint8_t target_system;
	uint16_t command;
	uint8_t confimation;
	float param1;
	float param2;
	float param3;
	float param4;
	float param5;
	float param6;
	float param7;
}__attribute__ ((packed)) command_xyi_long_t;

typedef struct 
{
	uint8_t msgid;
	uint8_t id_uav_xyi;
	uint16_t command;
	uint8_t result;
}__attribute__ ((packed)) command_ack_t;

typedef struct 
{
	uint8_t msgid;
	uint8_t target_uav;
	uint8_t target_system;
	uint16_t count;	
}__attribute__ ((packed)) mission_count_t;


typedef struct 
{
	uint8_t msgid;
	uint8_t target_uav;
	uint8_t target_system;
	uint16_t count;
	uint8_t frame;
	float gps[];
}__attribute__ ((packed)) mission_items_t;

typedef struct 
{
	uint8_t msgid;
	uint8_t target_uav;
	uint8_t target_system;
	uint8_t type;
}__attribute__ ((packed)) mission_ack_t;

typedef struct 
{
	uint8_t msgid;
	uint8_t target_uav;
	uint8_t target_system;
	uint8_t mode;
}__attribute__ ((packed)) mission_inq_t;

typedef struct 
{
	uint8_t msgid;
	uint8_t target_uav;
	uint8_t target_system;
	uint8_t type;
	uint16_t count;
	uint8_t frame;
	float gps[];
}__attribute__ ((packed)) mission_ack_route_t;

typedef struct 
{
	uint8_t msgid;
	uint8_t target_uav;
	uint8_t target_system;
	uint8_t type;
	uint16_t count;
	float gps[];
}__attribute__ ((packed)) mission_ack_emer_t;

typedef struct
{
	uint8_t msg_id;
	uint8_t id_uav_xyi;
	uint8_t id_iso_xyi;
	uint8_t base_mode;
	uint8_t system_status;	
	uint8_t xylink_version;
}__attribute__ ((packed)) hearbeat_t;

typedef struct 
{
	uint8_t msgid;
	int32_t target_uav;
	float cmd_status;
	float horizontal_control;
	float vertical_control;
	float offset_x_per;
	float offset_y_per;
	float offset_z_per;
}__attribute__ ((packed)) manual_clould_control_t;

typedef struct 
{
	uint8_t msgid;
	int32_t id_uav_xyi;
	unsigned char image[];
}__attribute__ ((packed)) emer_report_image_t;

/*********udp**********/
typedef struct 
{
	uint8_t msg_id;
	uint8_t id_uav_xyi;
	uint32_t time_boot_ms;
	uint16_t voltages[10];
	int16_t current_battery;
	int8_t battery_remaining;
}__attribute__ ((packed)) battery_status_t;

typedef struct 
{
	uint8_t msg_id;
	uint8_t id_uav_xyi;
	uint32_t time_boot_ms;
	float x;
	float y;
	float z;
	float vx;
	float vy;
	float vz;
	float dis_m;
}__attribute__ ((packed)) local_position_ned_t;

typedef struct 
{
	uint8_t msg_id;
	uint8_t id_uav_xyi;
	uint32_t time_boot_ms;
	int32_t relative_alt;
	uint16_t hdg;
}__attribute__ ((packed)) global_position_int_t;

typedef struct 
{
	uint8_t msg_id;
	uint8_t id_uav_xyi;
	uint32_t time_boot_ms;
	uint8_t fix_type;
	int32_t lat_gps;
	int32_t lon_gps;
	int32_t alt_gps;
	uint16_t eph;
	uint16_t epv;
	uint16_t vel_gps;
	uint16_t cog;
	uint8_t satellites_visible;
}__attribute__ ((packed)) gps_raw_t;

typedef struct udp_pack
{
	battery_status_t cur_battery;
	local_position_ned_t cur_local_pos;
	global_position_int_t cur_glob_pos;
	gps_raw_t cur_gps;
}udp_pack;

#ifndef HAVE_ENUM_COMMAND_TABLE
#define HAVE_ENUM_COMMAND_TABLE
typedef enum _COMMAND_TABLE
{
	XYI_CMD_START_TAKEOFF = 1,
	XYI_CMD_EMER_LAND,
	XYI_CMD_PID_TEST,
	XYI_CMD_MANUAL_STATUS_CTRL,
	XYI_CMD_H_CTRL,
	XYI_CMD_V_CTRL,
}COMMAND_TABLE;
#endif

#ifndef HAVE_ENUM_COMMAND_ACK_RESULT
#define HAVE_ENUM_COMMAND_ACK_RESULT
typedef enum _COMM_ACK_RESULT
{
	XYI_RESULT_ACCEPTED = 0,
	XYI_RESULT_TEMPERORARILY_REJECTED,
	XYI_RESULT_DENIED,
	XYI_RESULT_UNSUPPORTED,
	XYI_RESULT_FAILED,
}COMM_ACK_RESULT;
#endif

#ifndef HAVE_ENUM_MISSION_RESPONSE
#define HAVE_ENUM_MISSION_RESPONSE
typedef enum _MISSION_RESPONSE
{
	XYI_MISSION_ACCEPTED = 0,
	XYI_MISSION_ERROR,
	XYI_MISSION_UNSUPPORTED_FRAME,
	XYI_MISSION_UNSUPPORTED,
	XYI_MISSION_NO_SPACE,
	XYI_MISSION_INVALID_PARAM1,
	XYI_MISSION_INVALID_PARAM2,
	XYI_MISSION_DENIED,
	XYI_MISSION_GET_PARAM,
}MISSION_RESPONSE;
#endif


struct MemoryStruct {
  char *memory;
  size_t size;
};


class xy_command_c
{
public:
	xy_command_c(autopilot_interface_c &_pilot);
	~xy_command_c();

	autopilot_interface_c &pilot;
	mission_items_t *items;
	mission_ack_t miask;
	command_ack_t cask;
	mission_ack_route_t *p_rask;
	mission_ack_emer_t *p_eask;
	emer_report_image_t *emer_image;
	hearbeat_t cur_heartbeat;
	int ask_flag;
	int route_flag;




	void msg_pack(udp_pack *udp);

};

void* start_network_interface_send_thread(void *args);
void* start_network_interface_receive_thread(void *args);


class xy_network_c
{
public:
	xy_network_c(autopilot_interface_c &_pilot);
	~xy_network_c();
	
	char reading_status;
	char writing_status;
	xy_command_c mission_command;
	void start(void);

	int XY_Net_Reported_Setup(void);
	void start_send_thread(void);

private:
	pthread_t read_tid;
	pthread_t write_tid;
	
	int sock_fd;
	int new_link;
	sem_t report_drone_pos_sem;
	char http_post_str[HTTP_POST_DATA_SIZE];
	
	void reported_data_thread_func();

	int udp_report(int *sock, struct sockaddr_in *gcAddr);
	int report_command(void);
	
};

#endif


